
/* 
 * This file is part of FreeBMS - an open source battery management system
 *
 * CAN-Enabled Supervisor Microcontroller - PIC18F2680
 * 
 * NOTE: This code is designed to be compiled using SOURCEBOOST (http://www.sourceboost.com)
 */
 
/* This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* 
 * PORT A:
 *
 * | PIN | I/O | Description
 * -----------------------------------
 * | 0   | I   | AD Converter
 * | 1   | I   | AD Converter
 * | 2   | I   | AD Converter
 * | 3   | I   | AD Converter
 * | 4   | O   | Error LED
 * | 5   | I   | AD Converter
 * | 6   | n/a | n/a
 * | 7   | n/a | n/a
 *
 * PORTB:
 *
 * | PIN | I/O | Description
 * -----------------------------------
 * | 0   | n/a | n/a
 * | 1   | n/a | n/a
 * | 2   | O   | CANTX
 * | 3   | I   | CANRX
 * | 4   | O   | CHARGE RELAY
 * | 5   | O   | DISCHARGE RELAY
 * | 6   | n/a | ICSP Programming
 * | 7   | n/a | ICSP Programming
 * 
 * PORTC:
 * | PIN | I/O | Description
 * -----------------------------------
 * | 0   | O   | METER LED3
 * | 1   | O   | METER LED2
 * | 2   | O   | BUZZER (PWM channel 0)
 * | 3   | O   | METER LED1
 * | 4   | O   | METER LED4
 * | 5   | O   | EXTERNAL WATCHDOG
 * | 6   | O   | SERIAL TX
 * | 7   | I   | SERIAL RX
 *
 */

// Timer preloaders
#define TIMER0_PRELOAD_HI	0x0B	// Timer preloader for timer0: 100ms interval
#define TIMER0_PRELOAD_LO	0xDC	// Timer preloader for timer0: 100ms interval
#define TIMER1_PRELOAD_HI	0x3C	// Timer preloader for timer1: 10ms interval
#define TIMER1_PRELOAD_LO	0xB3	// Timer preloader for timer1: 10ms interval

// Contactor mappings
#define CHARGE_CONTACTOR	4		// RB4: Charge contactor relay: charger
#define DISCHARGE_CONTACTOR	5		// RB5: Discharge contactor relay: paralleling

// Misc mappings
#define WATCHDOG_PIN		5		// PORTC - External watchdog

// LED mappings
#define LED_ERROR			4		// PORTA - Error LED
#define LED_METER1			3		// PORTC - Battery SOC Meter LED 1
#define LED_METER2			1		// PORTC - Battery SOC Meter LED 1
#define LED_METER3			0		// PORTC - Battery SOC Meter LED 1
#define LED_METER4			4		// PORTC - Battery SOC Meter LED 1

// Operation modes
#define OPMODE_NORMAL		0		// Normal chip execution
#define OPMODE_DEBUG		1		// Debug mode, go through a series of tests to verify BMS
#define OPMODE_CONFIG		2		// Chip ceases normal work and waits for configuration
#define OPMODE_ERROR		3		// Chip has experienced a critical error and operation has halted
#define OPMODE_NOPOLL		4		// Chip no longer polls cells

// Global Includes 
#include <system.h>
#include <string.h>
#include "types.h"
#include "eeprom.h"
#include "canbus.h"
#include "serial.h"
#include "codes.h"
#include "struct.h"
#include "dtc.h"
#include "options.h"

// Configuration bits
#pragma DATA _CONFIG1H, _OSC_HSPLL_1H				// HS + PLL Oscillator (4x10 MHz)
#pragma DATA _CONFIG2H, _WDT_OFF_2H & _WDTPS_256_2H // Watchdog timer controlled by software
													// Postscaler: 256, lower to speed up
#pragma DATA _CONFIG2L, _BORV_2_2L				 	// Brown-out reset enable, 2.2V trigger
													// Power up timer enabled
#pragma DATA _CONFIG4L, _STVREN_ON_4L & _LVP_OFF_4L	&_XINST_OFF_4L
													// Stack-overflow chip reset enable
													// Disable low voltage programming
													// Disable extended instruction set
#pragma DATA _CONFIG5L, _CP0_OFF_5L & _CP1_OFF_5L	// Code protect off
#pragma DATA _CONFIG5H, _CPB_OFF_5H & _CPD_OFF_5H	// Code protect boot off
#pragma DATA _CONFIG6L, _WRT0_OFF_6L & _WRT1_OFF_6L	// Table write protect off
#pragma DATA _CONFIG6H, _WRTB_OFF_6H & _WRTC_OFF_6H	& _WRTD_OFF_6H          
#pragma DATA _CONFIG7L, _EBTR0_OFF_7L & _EBTR1_OFF_7L	
#pragma DATA _CONFIG7H, _EBTRB_OFF_7H
	
// Global Variables
struct cell_data *lowest_ptr;			// Pointer to lowest cell in pack
struct cell_data *highest_ptr;			// Pointer to highest cell in pack
unsigned long battery_ampsecs;			// Running ampere seconds counter
unsigned long battery_amphours;			// Running ampere hours counter
unsigned long battery_ampfull;			// Ampere hours when battery is full
unsigned long battery_ampempty;			// Ampere hours when battery is empty
unsigned long battery_amperr;			// Current error percentage for ADC
unsigned long battery_voltage;			// Current voltage of entire battery pack
unsigned long battery_resistance;		// Internal resistance of battery pack
unsigned short lowest_voltage;			// Lowest voltage detected in pack
unsigned short highest_voltage;			// Highest voltage detected in pack
unsigned short timer_pause;				// Duration to wait inbetween shunting cycles
unsigned short pause_duration;			// Duration inbetween each shunting cycle - programmable
unsigned short timer_shunt;				// Duration of each shunting cycle
unsigned short shunt_duration;			// Duration of each shunting cycle - programmable
unsigned short max_voltage;				// Maximum cell voltage, cutoff charger at this value
unsigned short min_voltage;				// Minimum cell voltage, cutoff discharge at this value
unsigned short diff_voltage;			// Keep battery voltages within a margin of this AD value
unsigned short balance_voltage;			// Minimum voltage for lowest cell to begin balancing at
unsigned short num_cells;				// Number of regulator boards to keep track of
unsigned short timer_errors;			// Timeout for cell / comm errors
BYTE timer_relay;						// Timer for relays turning off and measuring for current
BYTE timer_opmode;						// Timer for delaying changing operation mode
BYTE timer_watchdog;					// Timer for external watchdog
BYTE ms10_tick;							// Counter for 10ms timer ticks
BYTE ampere_tick;						// Counter for ampere hour calculations
BYTE ampere_count;						// Counter for number of ampere checks
BYTE operation_mode;					// Current operational mode of chip
BYTE requested_opmode;					// Requested opmode to change to once timer is zero
BYTE battery_soc;						// Estimated state of charge of the battery pack
BYTE comm_errors;						// Number of communication errors over serial
BYTE cell_errors;						// Number of invalid regulator submissions (0v, etc)
BYTE shunting_enabled;					// Whether or not we are performing shunting requests
BYTE baudrate;							// Serial baudrate (0: 2400, 1: 9600, etc)
BYTE ledscale;							// LED auxillary variable
BYTE ledstate;							// LED auxillary variable
bool start_balancing;					// Whether or not we have started balancing
bool done_balancing;					// Whether or not we are done balancing
bool charge_contactor;					// Whether or not charge contactor is set
bool discharge_contactor;				// Whether or not discharge contactor is set
bool charging;							// Whether or not we are charging the pack
bool discharging;						// Whether or not we are discharging the pack
bool calibrate_sensor;					// Whether or not to calibrate the current sensor
bool restart_collection;				// Boolean to restart data collection process
bool error_config;						// Boolean to determine if error occurred during config
bool full_run;							// Boolean to determine if we have made it through all regulators

// Regulator configuration variables
unsigned short reg_safetytimer;			// Regulator safety timer for shunting
unsigned short reg_maxvoltage;			// Regulator safety maximum voltage
BYTE reg_adcdelay;						// Regulator ADC delay value
BYTE reg_ledmode;						// Regulator LED configuration mode

// Broadcast message buffers
BYTE bcast1_100ms[8];					// Broadcast 1: Highest voltage, lowest voltage, status
BYTE bcast2_100ms[8];					// Broadcast 2: Voltage, SOC, temps, relays

// Local shadow variables
BYTE localporta;						// Local shadow of porta
BYTE localportb;						// Local shadow of portb
BYTE localportc;						// Local shadow of portc
BYTE localtrisa;						// Local shadow of trisa
BYTE localtrisb;						// Local shadow of trisb
BYTE localtrisc;						// Local shadow of trisc

// ADC Bits
volatile bit adc_go @ ADCON0 . GO;
volatile bit adc_on @ ADCON0 . ADON;

// Function prototypes
char eeprom_read(char address);
void eeprom_write(char address, char data);
void calibrate_sensor();

// High priority interrupt handler
void interrupt(void) {
	unsigned long NewMessage, sum;
	BYTE NMD[8]; // Buffer for new message
	BYTE NewMessageLen;
	BYTE chip;
	BYTE i, checksum;
	BYTE mesg_work;
	enum CAN_RX_MSG_FLAGS NewMessageFlags;
	struct cell_data *tmp_ptr;
	struct dtc_data *dtc_ptr;

	// Clear DTC button pressed
#if CLEARBUTTON_ENABLED==1
	if(test_bit(intcon, INT0IF)) {
		// Clear interrupt flag
		clear_bit(intcon, INT0IF);
				
/*	
		// Set DTC priority to low to avoid buzzer and LED
		for(dtc_ptr = first_dtc; dtc_ptr; dtc_ptr = dtc_ptr->next) {
			// Clear watchdog timer
			clear_wdt();
			
			// Mark all DTCs as low priority
			dtc_ptr->data[1] = DTC_PRIORITY_LOW;

					NMD[0] = dtc_ptr->data[0];
					NMD[1] = dtc_ptr->data[1];
					
					CANSendMessage_inthi( 0x7EE, NMD, 2, CAN_TX_PRIORITY_2 & 
														CAN_TX_STD_FRAME & 
														CAN_TX_NO_RTR_FRAME);

		}

		// Turn off error LED
		clear_bit(localporta, LED_ERROR);
		porta = localporta;

		// Clear DTC booleans
		init_dtc();

		// Turn off the buzzer
		pr2 = 0x00;
		t2con = 0x00;
		ccp1con = 0x00;	

		// Delay before reset
		delay_ms(50);

		// Reset the chip to avoid unpleasent effects
		reset();
*/

		// Clear DTCs
		eeprom_write(EEPROM_DTC_NUM, 0x00);
		delay_ms(50);
	
		// Resetting the chip takes care of active DTCs
		asm
		{
			RESET;
		}
	}	
#else
	#warning "Clear DTC button disabled."
#endif // CLEARBUTTON_ENABLED

	// Serial receive message
	if(test_bit(pir1, RCIF)) {

		// Receive and parse message
		if(SerialReceiveMessage()) {

			// Clear watchdog timer
			clear_wdt();
			
			// Parse request
			switch(rxbuffer[2]) {

				// Regulator has submitted battery voltage
				case ID_VOLT:
					unsigned int tmp_voltage; // Temporary voltage variable

					// Pull the voltage from the rxbuffer
					tmp_voltage = (rxbuffer[3] << 8) | rxbuffer[4];
					
					// Syntax: update_voltage(id, shunting, voltage);
					tmp_ptr = update_voltage(rxbuffer[1], rxbuffer[5], tmp_voltage);

#if CELLBCAST_ENABLED==1
					// Broadcast voltage for each cell
					NMD[0] = rxbuffer[1];
					NMD[1] = rxbuffer[3];
					NMD[2] = rxbuffer[4];
					NMD[3] = rxbuffer[5];
					
					// Calculate checksum
					checksum = CAN_ID_CELLBCAST;
					for(i = 0; i < 4; ++i)
						checksum += NMD[i];
					checksum &= 0xFF;
					NMD[4] = (BYTE) checksum;
					
					CANSendMessage_inthi( CAN_ID_CELLBCAST, NMD, 5, CAN_TX_PRIORITY_2 & 
														CAN_TX_STD_FRAME & 
														CAN_TX_NO_RTR_FRAME);
#endif

					// Check to see if voltage is an invalid result
					if(tmp_voltage >= 0xFF00) {
					
						// Increment total cell errors if we are in normal mode
						if(operation_mode == OPMODE_NORMAL)
							++cell_errors;

						// Start the timeout timer for errors
						if(timer_errors <= 0)
							timer_errors = TIMER_ERRORS;

						// Build message for DTC
						NMD[0] = ERROR_CELL_FAULT;
						NMD[1] = DTC_PRIORITY_MED;
						NMD[2] = rxbuffer[1];
						NMD[3] = 0;
						NMD[4] = rxbuffer[3];
						NMD[5] = rxbuffer[4];

						// Loop through cells to find ID
						for(tmp_ptr = first_cell; tmp_ptr; tmp_ptr = tmp_ptr->next) {
							if(tmp_ptr->id == rxbuffer[1]) {
								// Add to cell error count for specific cell
								tmp_ptr->cell_errors++;
								// Report number of cell errors if we found the cell ID
								NMD[3] = tmp_ptr->cell_errors;
								break;
							}
						}

						// Update the DTC, if it doesn't exist it will insert a new one
						if(cell_errors >= MAX_CELL_ERRORS)
							update_dtc(6, NMD);

						// Toggle flag to request restart in data collection
						restart_collection = true;

					} else {
						// Update highest and lowest voltage records
						if(tmp_voltage > highest_voltage) {
							highest_ptr = tmp_ptr;
							highest_voltage = tmp_voltage;
						} else if(tmp_voltage < lowest_voltage && tmp_voltage > 0) { 
							lowest_ptr = tmp_ptr;
							lowest_voltage = tmp_voltage;
						}
					
						// Update battery_voltage
						battery_voltage += tmp_voltage;
					}
					break;

				// Regulator has submitted acknowledgement for shunting
				case ID_SHUNT:
					// Update shunting status for cell
					tmp_ptr = update_shunting(rxbuffer[1], rxbuffer[3]);
					break;

				// Regulator has submitted acknowledgement for reprogramming safety timers
				case ID_PROGRAM_TIMERS:
					// If value returned does not equal the value we sent, trigger error
					if(((rxbuffer[3] << 8) | rxbuffer[4]) != reg_safetytimer)
						error_config = true;
					break;

				// Regulator has submitted acknowledgement for reprogramming ADC delay
				case ID_PROGRAM_LEDS:
					// If value returned does not equal the value we sent, trigger error
					if(rxbuffer[3] != reg_ledmode)
						error_config = true;
					break;

				// Regulator has submitted acknowledgement for reprogramming ADC delay
				case ID_PROGRAM_DELAY:
					// If value returned does not equal the value we sent, trigger error
					if(rxbuffer[3] != reg_adcdelay)
						error_config = true;
					break;

				// Regulator has submitted acknowledgement for reprogramming voltage cutoffs
				case ID_PROGRAM_CUTOFFS:
					// If value returned does not equal the value we sent, trigger error
					if(((rxbuffer[3] << 8) | rxbuffer[4]) != reg_maxvoltage)
						error_config = true;
					break;

				// Regulator has submitted acknowledgement for reprogramming error percentage
				case ID_PROGRAM_ERROR:
					// Send response back to the CANBUS
					NMD[0] = CONFIG_REG_ERROR;
					NMD[1] = rxbuffer[1];
					NMD[2] = rxbuffer[3];
					NMD[3] = rxbuffer[4];
					NMD[4] = rxbuffer[5];
					
					// Add message to configuration message queue
					insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 5, NMD);					
					break;
			}
		}		

		// Clear serial receive interrupt
		clear_bit(pir1, RCIF);
	}
	
	// Check if we were interrupted because of a received CAN message
	if ( CANIsRxReady() )
	{
		// Receive CANBUS message for high and low priority buffers
		CANReceiveMessage(&NewMessage, NMD, &NewMessageLen, &NewMessageFlags);

		if ( NewMessageFlags & CAN_RX_OVERFLOW ) {} // RX overflow occurred
		else if ( NewMessageFlags & CAN_RX_INVALID_MSG ) {} // Invalid message received
		else if ( NewMessageFlags & CAN_RX_XTD_FRAME ) {} // Extended Identifier received
		else if ( NewMessageFlags & CAN_RX_RTR_FRAME ) {} // RTR frame received (Request?)
		else // Standard Identifier received
		{
			// Check if we received a DTC handler message
			if(NewMessage == CAN_ID_DTC) {
				switch(NMD[0]) {
					// Load latched DTCs into active memory
					case ERROR_LOAD_DTCS:
						if(NMD[1] == 1) {
							load_dtcs(false);
						}
						break;

					// Clear active and latched DTCs
					case ERROR_CLEAR_DTCS:
						if(NMD[1] == 1) {
							clear_dtcs();
						}
						break;
				}
			}
			// Check if we received a BMS configuration message
			// Only accept message if its changing OPMODE or we are already in reprogramming mode
			else if(NewMessage == CAN_ID_CONFIG
			&& (NMD[0] == CONFIG_OPMODE || operation_mode == OPMODE_CONFIG)) {

				// Switch on the type of configuration message
				switch(NMD[0]) {
				
					// Change operational mode
					case CONFIG_OPMODE:
						// Get new operation mode
						requested_opmode = NMD[1];

						// If new operation mode is invalid, set to default
						if(requested_opmode > OPMODE_NOPOLL)
							requested_opmode = OPMODE_NORMAL;

						// Insert delay before returning to normal operation mode
						if(requested_opmode == OPMODE_NORMAL) {
							timer_opmode = TIMER_OPMODE;
						} else { // Immediately go to requested mode
							operation_mode = requested_opmode;
						}

						// Send acknowledgement response that we have queued mode change
						NMD[0] = CONFIG_OPMODE;
						NMD[1] = requested_opmode;

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						break;

					/* 
					 * Supervisor board configuration requests
					 */
				
					// Calibrate current sensor
					case CONFIG_SENSORCALIB:
						if(NMD[1] == 1) {
							calibrate_sensor = true;
						}
						break;

					// Whether battery balancing is enabled or disabled
					case CONFIG_SHUNTING:
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_SHUNTING;
							NMD[1] = NMD[2];
	
							// Update local copy
							shunting_enabled = NMD[1];

							// Begin balancing if it was enabled
							start_balancing = NMD[1];

							// Write new value to EEPROM
							eeprom_write(EEPROM_SHUNTING, shunting_enabled);		
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_SHUNTING;
							NMD[1] = shunting_enabled;
						}
						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						break;

					// Change number of regulator cells
					case CONFIG_NUMCELLS:
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_NUMCELLS;
							NMD[1] = NMD[2];
	
							// Update local copy
							num_cells = NMD[1];
				
							// Write new value to EEPROM
							eeprom_write(EEPROM_NUMCELLS, num_cells);		
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_NUMCELLS;
							NMD[1] = num_cells;
						}
						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						break;
						
					// Change serial baudrate configuration
					case CONFIG_BAUDRATE:
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_BAUDRATE;
							NMD[1] = NMD[2];
	
							// Update local copy
							baudrate = NMD[1];
				
							// Write new value to EEPROM
							eeprom_write(EEPROM_BAUDRATE, baudrate);		
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_BAUDRATE;
							NMD[1] = baudrate;
						}
						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						break;
						
					// Change minimum voltage cutoff
					case CONFIG_MINVOLT:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety voltage cutoff
							min_voltage = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_MINVOLT_HI, ((min_voltage >> 8) & 0x00FF));
							eeprom_write(EEPROM_MINVOLT_LO, (min_voltage & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_MINVOLT;
							NMD[1] = ((min_voltage << 8) & 0x00FF);
							NMD[2] = (min_voltage & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change maximum voltage cutoff
					case CONFIG_MAXVOLT:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety voltage cutoff
							max_voltage = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_MAXVOLT_HI, ((max_voltage >> 8) & 0x00FF));
							eeprom_write(EEPROM_MAXVOLT_LO, (max_voltage & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_MAXVOLT;
							NMD[1] = ((max_voltage << 8) & 0x00FF);
							NMD[2] = (max_voltage & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change differencial voltage for shunting
					case CONFIG_DIFFVOLT:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety voltage cutoff
							diff_voltage = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_DIFFVOLT_HI, ((diff_voltage >> 8) & 0x00FF));
							eeprom_write(EEPROM_DIFFVOLT_LO, (diff_voltage & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_DIFFVOLT;
							NMD[1] = ((diff_voltage << 8) & 0x00FF);
							NMD[2] = (diff_voltage & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change balancing voltage for shunting
					case CONFIG_BALVOLT:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety voltage cutoff
							balance_voltage = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_BALVOLT_HI, ((balance_voltage >> 8) & 0x00FF));
							eeprom_write(EEPROM_BALVOLT_LO, (balance_voltage & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_BALVOLT;
							NMD[1] = ((balance_voltage << 8) & 0x00FF);
							NMD[2] = (balance_voltage & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change pause timer duration inbetween shunting requests
					case CONFIG_TIMERPAUSE:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy
							pause_duration = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_TIMERPAUSE_HI, ((pause_duration >> 8) & 0x00FF));
							eeprom_write(EEPROM_TIMERPAUSE_LO, (pause_duration & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_TIMERPAUSE;
							NMD[1] = ((pause_duration << 8) & 0x00FF);
							NMD[2] = (pause_duration & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change shunting duration period timer
					case CONFIG_TIMERSHUNT:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy
							shunt_duration = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_TIMERSHUNT_HI, ((shunt_duration >> 8) & 0x00FF));
							eeprom_write(EEPROM_TIMERSHUNT_LO, (shunt_duration & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_TIMERSHUNT;
							NMD[1] = ((shunt_duration << 8) & 0x00FF);
							NMD[2] = (shunt_duration & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						break;

					// Change current SOC
					case CONFIG_BATTERYSOC:
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_BATTERYSOC;
							NMD[1] = NMD[2];
	
							// Update local copy
							battery_soc = NMD[1];
				
							// Write new value to EEPROM
							eeprom_write(EEPROM_SOC, battery_soc);		
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_BATTERYSOC;
							NMD[1] = battery_soc;
						}
						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						break;

					// Change current amphour counter value
					case CONFIG_AMPHOURS:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_AMPHOURS;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
							NMD[3] = NMD[4];
							NMD[4] = NMD[5];
	
							// Update local copy of regulator safety voltage cutoff
							battery_amphours = ((NMD[1] << 24) | (NMD[2] << 16) | (NMD[3] << 8) | NMD[4]);
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_AMPERE_CUR_B0, ((battery_amphours >> 24) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_CUR_B1, ((battery_amphours >> 16) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_CUR_B2, ((battery_amphours >> 8) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_CUR_B3, (battery_amphours & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_AMPHOURS;
							NMD[1] = ((battery_amphours >> 24) & 0x00FF);
							NMD[2] = ((battery_amphours >> 16) & 0x00FF);
							NMD[3] = ((battery_amphours >> 8) & 0x00FF);
							NMD[4] = (battery_amphours & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 5, NMD);
						break;

					// Change full amphour counter value
					case CONFIG_AMPFULL:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_AMPFULL;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
							NMD[3] = NMD[4];
							NMD[4] = NMD[5];
	
							// Update local copy of regulator safety voltage cutoff
							battery_ampfull = ((NMD[1] << 24) | (NMD[2] << 16) | (NMD[3] << 8) | NMD[4]);
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_AMPERE_FULL_B0, ((battery_ampfull >> 24) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_FULL_B1, ((battery_ampfull >> 16) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_FULL_B2, ((battery_ampfull >> 8) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_FULL_B3, (battery_ampfull & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_AMPFULL;
							NMD[1] = ((battery_ampfull >> 24) & 0x00FF);
							NMD[2] = ((battery_ampfull >> 16) & 0x00FF);
							NMD[3] = ((battery_ampfull >> 8) & 0x00FF);
							NMD[4] = (battery_ampfull & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 5, NMD);
						break;

					// Change empty amphour counter value
					case CONFIG_AMPEMPTY:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = CONFIG_AMPEMPTY;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
							NMD[3] = NMD[4];
							NMD[4] = NMD[5];
	
							// Update local copy of regulator safety voltage cutoff
							battery_ampempty = ((NMD[1] << 24) | (NMD[2] << 16) | (NMD[3] << 8) | NMD[4]);
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_AMPERE_EMPTY_B0, ((battery_ampempty >> 24) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_EMPTY_B1, ((battery_ampempty >> 16) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_EMPTY_B2, ((battery_ampempty >> 8) & 0x00FF));
							eeprom_write(EEPROM_AMPERE_EMPTY_B3, (battery_ampempty & 0x00FF));
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_AMPEMPTY;
							NMD[1] = ((battery_ampempty >> 24) & 0x00FF);
							NMD[2] = ((battery_ampempty >> 16) & 0x00FF);
							NMD[3] = ((battery_ampempty >> 8) & 0x00FF);
							NMD[4] = (battery_ampempty & 0x00FF);
						}

						// Add message to configuration message queue
						insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 5, NMD);
						break;

					/*
					 * Regulator board configuration requests
					 */
						
					// Request that a specific regulator shunt
					case CONFIG_REG_SHUNTREQ:
						// Set new message buffer and send message
						NMD[0] = NMD[2];

						// Add message to configuration message queue
						insert_mesg(MESG_SERIAL, ID_SHUNT, NMD[1], true, 1, NMD);
						break;
										
					// Change the LED mode for regulators
					case CONFIG_REG_LEDMODE:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
	
							// Update local copy of regulator ADC delay value
							reg_ledmode = NMD[1];
				
							// Write new value to EEPROM
							eeprom_write(EEPROM_REG_LEDMODE, reg_ledmode);		

							// Add message to configuration message queue
							insert_mesg(MESG_SERIAL, ID_PROGRAM_LEDS, 0x00, true, 2, NMD);
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_REG_LEDMODE;
							NMD[1] = reg_ledmode;
	
							// Add message to configuration message queue
							insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						}
						break;

					// Program an ADC delay value
					case CONFIG_REG_ADCDELAY:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							
							// Update local copy of regulator ADC delay value
							reg_adcdelay = NMD[1];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_REG_ADCDELAY, reg_adcdelay);
							
							// Add message to configuration message queue
							insert_mesg(MESG_SERIAL, ID_PROGRAM_DELAY, 0x00, true, 2, NMD);
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_REG_ADCDELAY;
							NMD[1] = reg_adcdelay;
	
							// Add message to configuration message queue
							insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 2, NMD);
						}
						break;

					// Program cutoff voltages
					case CONFIG_REG_CUTOFFS:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety voltage cutoff
							reg_maxvoltage = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_REG_MAXVOLT_HI, ((reg_maxvoltage >> 8) & 0x00FF));
							eeprom_write(EEPROM_REG_MAXVOLT_LO, (reg_maxvoltage & 0x00FF));
	
							// Add message to configuration message queue
							insert_mesg(MESG_SERIAL, ID_PROGRAM_CUTOFFS, 0x00, true, 3, NMD);
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_REG_CUTOFFS;
							NMD[1] = ((reg_maxvoltage << 8) & 0x00FF);
							NMD[2] = (reg_maxvoltage & 0x00FF);
	
							// Add message to configuration message queue
							insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						}
						break;

					// Program safety timers
					case CONFIG_REG_TIMERS:
						// If byte1 equals 1, set new value otherwise report current value
						if(NMD[1] == 1) {
							// Set new message buffer and send message
							NMD[0] = 1;
							NMD[1] = NMD[2];
							NMD[2] = NMD[3];
	
							// Update local copy of regulator safety shunting timer
							reg_safetytimer = (NMD[1] << 8) | NMD[2];
	
							// Write new value to EEPROM
							eeprom_write(EEPROM_REG_STIMER_HI, ((reg_safetytimer >> 8) & 0x00FF));
							eeprom_write(EEPROM_REG_STIMER_LO, (reg_safetytimer & 0x00FF));
	
							// Add message to configuration message queue
							insert_mesg(MESG_SERIAL, ID_PROGRAM_TIMERS, 0x00, true, 3, NMD);
						} else {
							// Build response for programmer
							NMD[0] = CONFIG_REG_TIMERS;
							NMD[1] = ((reg_safetytimer << 8) & 0x00FF);
							NMD[2] = (reg_safetytimer & 0x00FF);
	
							// Add message to configuration message queue
							insert_mesg(MESG_CANBUS, CAN_ID_RESPONSE, 0x00, true, 3, NMD);
						}
						break;
						
					// Default case -- ignore message
					default:
						break;
				}
			} 
		}
	}	
	return;
}

// Low priority interrupt handler
void interrupt_low(void) {
	BYTE NMD[8]; // Buffer for new message
	BYTE i, checksum;

	// Check if we were interrupted because of timers
	if(test_bit(pir1, TMR1IF)) { // Timer1: 10ms
#if SENSOR_ENABLED==1
		unsigned long cur_work;		// Current temp variable
		unsigned long cur_current; 	// Current ADC convert value
		signed short cur_bcast;		// Current broadcast variable
		signed short amphour_bcast; // Amphour broadcast variable 
		bool cur_neg;				// Current charge/discharge variable

		// Clear the channel selection in adcon0 and set the requested one
		adcon0 &= 11000011b;
		adcon0 |= (0x00 & 0x15) << 2;

		// Wait the required acquisition time (datasheet)
		delay_10us(2);

		// Start the conversion
		adc_go = 1;

		// Wait until it is done
		while (adc_go);

		// Store in local variable
		cur_current = (short)adresh;
		cur_current = cur_current << 8;
		cur_current |= adresl;

//#warning "AD Converter results still being published"
// TEMP DEBUG: Broadcast AD converter results
#if ADC_DEBUG==1
NMD[0] = (char)((cur_current >> 8) & 0x00FF);
NMD[1] = (char)(cur_current & 0x00FF);
NMD[2] = (char)((battery_amperr >> 24) & 0x00FF);
NMD[3] = (char)((battery_amperr >> 16) & 0x00FF);
NMD[4] = (char)((battery_amperr >> 8) & 0x00FF);
NMD[5] = (char)(battery_amperr & 0x00FF);
CANSendMessage_intlo( 0x1C, NMD, 6, CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME);
#warning "ADC debugging enabled."
#endif

		// Trigger error_sensorfault if bogus value detected at ADC
		if(cur_current <= 100 && error_sensorfault == false) {
			error_sensorfault = true;
			
			// Build message for DTC
			NMD[0] = ERROR_SENSOR_FAULT;
			NMD[1] = DTC_PRIORITY_MED;
			NMD[2] = (cur_current >> 8) & 0xFF;
			NMD[3] = (cur_current) & 0xFF;

			// Update the DTC, if it doesn't exist it will insert a new one
			update_dtc(4, NMD);
		}
/*
		// If we are calibrating the sensor, store current ADC value as zero
		if(calibrate_sensor == true) {
		
#warning "Probably not a good idea to have this here...figure out how to get around this"
			// Allow for voltage to stabilize
			clear_wdt();
			delay_ms(100);
			clear_wdt();
		
			// Set the amperror coefficient to the current ADC value
			battery_amperr = cur_current;
			
			// Write new value to EEPROM
//			eeprom_write(EEPROM_AMPERE_ERR_HI, ((battery_amperr >> 8) & 0x00FF));
//			eeprom_write(EEPROM_AMPERE_ERR_LO, (battery_amperr & 0x00FF));

			// Multiply by 10,000 to avoid floating point
			battery_amperr = battery_amperr * 10000;

			// Report new error value to the CANBUS
			NMD[0] = CONFIG_SENSORCALIB;
			NMD[1] = (BYTE)((battery_amperr >> 8) & 0x00FF);
			NMD[2] = (BYTE)(battery_amperr & 0x00FF);
			CANSendMessage_intlo( CAN_ID_RESPONSE, NMD, 3, CAN_TX_PRIORITY_0 
												& CAN_TX_STD_FRAME 
												& CAN_TX_NO_RTR_FRAME);

			// Clear the calibration flag
			calibrate_sensor = false;
		}
*/
		// Multiply by 10,000 to avoid floating point
		cur_current *= 10000;

		// Determine if we are charging or discharging battery pack
		if(cur_current > battery_amperr) {
			cur_neg = true;
			cur_work = ((cur_current - battery_amperr) / 1024) * 5;
		} else {
			cur_neg = false;
			cur_work = ((battery_amperr - cur_current) / 1024) * 5;
		}

		// Final current result (cur_work * (2000 / 15));
		cur_current = cur_work * 133;

		// Record whether we are charging or not for the LED battery meter
		if(cur_neg == true) {
			charging = false;
			discharging = true;
		} else {
			charging = true;
			discharging = false;
		}

		// 3.0 Amp tolerance
		if(cur_current < 30000 || error_sensorfault == true) {
			discharging = false;
			charging = false;
			cur_current = 0;
			cur_neg = false;
		}

		// Begin relay DTC evaluation
		if(timer_relay <= 0) {
			if(cur_current > 0 && ((discharge_contactor == true && cur_neg == true)
			|| (charge_contactor == true && cur_neg == false))) {
				if(error_relayfault == true) {
					// Set a relay DTC
					NMD[0] = ERROR_RELAY_FAULT;
					NMD[1] = DTC_PRIORITY_HI;

					// Reduce accuracy of current sensor value for 16bit transit
					cur_bcast = cur_current / 100;
	
					// Set value as negative if cur_neg is true
					if(cur_neg == true)
						cur_bcast = -cur_bcast;

					NMD[2] = (cur_bcast >> 8) & 0xFF;
					NMD[3] = cur_bcast & 0xFF;

					// Update the DTC, if it doesn't exist it will insert a new one
					update_dtc(4, NMD);	
				} else {
					error_relayfault = true;
					timer_relay = TIMER_RELAY;
				}
			} else {
				error_relayfault = false;
			}
		}

		// Add to running amp total...
		battery_ampsecs += cur_current;
		
		// Add to the running counter for ampere evaluations
		ampere_count++;
		
		// Update running total every 100 ampere ticks (1 second)
		if(ampere_tick >= 100) {
			ampere_tick = 0;

			// Avoid divide by zero below
			if(ampere_count <= 0) {
				ampere_count = 1;
			}

			// Convert to ampere hours
			battery_ampsecs /= ampere_count; // Divide by the number of times we polled
			battery_ampsecs /= 3600; // Divide by 3600 to get in terms of ampere hours

			// Add to (or subtract from) running ampere hour total
			if(charging == true) {
				battery_amphours += battery_ampsecs;
			} else if(discharging == true && battery_amphours > 0) {
				if(battery_amphours > battery_ampsecs) {
					battery_amphours -= battery_ampsecs;
				} else {
					battery_amphours = 0;
				}
			}

			// Zero out the ampere counter
			ampere_count = 0;

			// Zero out existing ampere second counter
			battery_ampsecs = 0;
		} else {
			// Increment ampere tick counter
			ampere_tick++;
		}

		// 50ms iteration
		if(ms10_tick >= 5) {
			// Check for divide by zero
			if(cur_current > 0) {
				// Reduce accuracy of current sensor value for 16bit transit
				cur_bcast = cur_current / 100;
	
				// Set value as negative if cur_neg is true
				if(cur_neg == true)
					cur_bcast = -cur_bcast;
			} else {
				cur_bcast = 0;
			}

			// Check for divide by zero
			if(battery_amphours > 0) {
				// Reduce accuracy of amphour value for 16bit transit
				amphour_bcast = battery_amphours / 100;
			} else {
				amphour_bcast = 0;
			}
			
			// Build first 50ms broadcast message
			// [Current In/Out (Signed)] [Ampere Count]
			NMD[0] = (BYTE)((cur_bcast >> 8) & 0x00FF);
			NMD[1] = (BYTE)((cur_bcast) & 0x00FF);
			NMD[2] = (BYTE)((amphour_bcast >> 8) & 0x00FF);
			NMD[3] = (BYTE)((amphour_bcast) & 0x00FF);
			
			// Calculate checksum - initialize to ID + length
			checksum = CAN_ID_BCAST3 + 0x04;
			for(i = 0; i < 4; ++i)
				checksum += NMD[i];
			checksum &= 0xFF;
			NMD[4] = (BYTE) checksum;
			
			// Verify that we are operating normally
			if(operation_mode != OPMODE_CONFIG) {
				// Send 50ms broadcast message
				CANSendMessage_intlo( CAN_ID_BCAST3, NMD, 5, CAN_TX_PRIORITY_1 & 
													CAN_TX_STD_FRAME & 
													CAN_TX_NO_RTR_FRAME);
			}
			
			// Reset timer-tick counter
			ms10_tick = 0;
		} else {
			// Increment timer-tick counter
			ms10_tick++;
		}
#else
		#warning "Current sensor disabled."
#endif // SENSOR_ENABLED

		// Decrement external watchdog timer
		if(timer_watchdog > 0)
			--timer_watchdog;
		else {
			// Clear external watchdog timer if no critical errors
			if(error_critical == false) {
				toggle_bit(localportc, WATCHDOG_PIN);
				timer_watchdog = TIMER_WATCHDOG;
				portc = localportc;
			}
		}

		// Preload value into timer1 for 10ms delay
		tmr1h = TIMER1_PRELOAD_HI;
		tmr1l = TIMER1_PRELOAD_LO;

		// Clear the interrupt
		clear_bit(pir1, TMR1IF);
	
	} else if(test_bit(intcon, TMR0IF)) { // Timer0: 100ms

		// Verify that we are operating normally
		if(operation_mode != OPMODE_CONFIG) {
			// Broadcast first 100ms interval CANBUS message
			CANSendMessage_intlo( CAN_ID_BCAST1, bcast1_100ms, 8, CAN_TX_PRIORITY_2 & 
													CAN_TX_STD_FRAME & 
													CAN_TX_NO_RTR_FRAME);
												
			// Broadcast second 100ms interval CANBUS message
			CANSendMessage_intlo( CAN_ID_BCAST2, bcast2_100ms, 8, CAN_TX_PRIORITY_1 & 
													CAN_TX_STD_FRAME & 
													CAN_TX_NO_RTR_FRAME);
		}

		// Increment LED scale counter
		if(ledscale >= 3)
			ledscale = 0;
		else
			ledscale = ledscale + 1;

		// Handle all software controlled LED activity
		if(ledscale == 0) {
			// Copy portc to local copy
			localportc = portc;

			// Flash error LED for serious errors
			if(error_memfault == true
			|| error_trisfault == true) {
				toggle_bit(localporta, LED_ERROR);
				porta = localporta;
			}

#if METER_ENABLED==1
			if(charging == true || battery_soc >= 100) {
				if(battery_soc < 100) { // Less than 100 percent battery_soc
					// Charging animation
					if(ledstate == 0) {
						clear_bit(localportc, LED_METER1);
						clear_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 1) {
						set_bit(localportc, LED_METER1);
						clear_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 2) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 3) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						set_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 4) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						set_bit(localportc, LED_METER3);
						set_bit(localportc, LED_METER4);
					}

					// Flash LEDs midway through so we can see SOC while charging
					if(battery_soc >= 75 && battery_soc < 100) {
						set_bit(localportc, LED_METER4);
					} else if(battery_soc >= 50 && battery_soc < 75) {
						set_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					}
				} else { // 100 percent
					toggle_bit(localportc, LED_METER1);
					localportc.LED_METER2 = localportc.LED_METER1;
					localportc.LED_METER3 = localportc.LED_METER1;
					localportc.LED_METER4 = localportc.LED_METER1;
				}	
			} else { // Not charging
				if(discharging == true) {
					// Discharging animation
					if(ledstate == 0) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						set_bit(localportc, LED_METER3);
						set_bit(localportc, LED_METER4);
					} else if(ledstate == 1) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						set_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 2) {
						set_bit(localportc, LED_METER1);
						set_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 3) {
						set_bit(localportc, LED_METER1);
						clear_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					} else if(ledstate == 4) {
						clear_bit(localportc, LED_METER1);
						clear_bit(localportc, LED_METER2);
						clear_bit(localportc, LED_METER3);
						clear_bit(localportc, LED_METER4);
					}
				} else if(battery_soc >= 100) {
					set_bit(localportc, LED_METER1);
					set_bit(localportc, LED_METER2);
					set_bit(localportc, LED_METER3);
					set_bit(localportc, LED_METER4);		
				} else if(battery_soc >= 75 && battery_soc < 100) {
					set_bit(localportc, LED_METER1);
					set_bit(localportc, LED_METER2);
					set_bit(localportc, LED_METER3);
					toggle_bit(localportc, LED_METER4);
				} else if(battery_soc >= 50 && battery_soc < 75) {
					set_bit(localportc, LED_METER1);
					set_bit(localportc, LED_METER2);
					toggle_bit(localportc, LED_METER3);
					clear_bit(localportc, LED_METER4);
				} else if(battery_soc >= 25 && battery_soc < 50) {
					set_bit(localportc, LED_METER1);
					toggle_bit(localportc, LED_METER2);
					clear_bit(localportc, LED_METER3);
					clear_bit(localportc, LED_METER4);	
				} else if(battery_soc > 0 && battery_soc < 25) {
					toggle_bit(localportc, LED_METER1);
					clear_bit(localportc, LED_METER2);
					clear_bit(localportc, LED_METER3);
					clear_bit(localportc, LED_METER4);	
				} else if(battery_soc <= 0) {
					clear_bit(localportc, LED_METER1);
					clear_bit(localportc, LED_METER2);
					clear_bit(localportc, LED_METER3);
					clear_bit(localportc, LED_METER4);
				}
			}
#endif

			// Update porta and portc with local copies
			porta = localporta;
			portc = localportc;

			// Update LED state
			if(ledstate >= 4)
				ledstate = 0;
			else
				ledstate = ledstate + 1;
		}

		// Decrement error timer
		if(timer_errors > 0) {
			--timer_errors;
		} else {
			cell_errors = 0;
			comm_errors = 0;
		}

		// Decrement opmode timer
		if(timer_opmode > 0)
			--timer_opmode;
		else
			operation_mode = requested_opmode;
		
		// If any contactors are on, decrement timer
		if(discharge_contactor == true || charge_contactor == true) {
			if(timer_relay > 0) {
				// Decrement relay timer
				--timer_relay;
			}
		}
		
		// Adjust timer values for shunting evaluations
		if(timer_shunt > 0)
			--timer_shunt;
		else {
			if(timer_pause > 0)
				--timer_pause;
		}
					
		// Decrement the rxblock_timer if its active, so we make sure to break out of loops
		if(rxblock_timer > 0) { rxblock_timer--; }

		// Preload timer1 for 100ms interval
		tmr0h = TIMER0_PRELOAD_HI;
		tmr0l = TIMER0_PRELOAD_LO;

		// Clear interrupt flag
		clear_bit(intcon, TMR0IF);
	}

	return;
}

// Main code
void main(void) {
	// Local variables
	struct cell_data *tmp_ptr;
	struct mesg_data *mesg_ptr;
	struct dtc_data *dtc_ptr;
	unsigned int target_board = 0;
	unsigned int volt_bcast = 0;
	bool shunt_sent = false;
	bool shunt_errors = false;
	bool buzzer_on = false;
	BYTE NMD[8]; // Buffer for sending messages
	BYTE record_id = 1;
	BYTE i = 0, checksum = 0;

	// Configure AD converters
//	adcon0 = 10000001b; // FOSC/64
//	adcon1 = 11000101b; // ADCS2 (bit 6) = 1
	adcon0 = 00000001b; // Enable AD conversions
	adcon1 = 00011010b; // AN0-AN4 as analog, rest are digital, AN3 as VREF+ 5v
	adcon2 = 10110110b; // FOSC/64, 16TAD, right justified
	
	// Trisa:	RA0 (0) - 1
	//			RA1 (1) - 1
	//			RA2 (2) - 1
	//			RA3 (3) - 1
	//		ERR LED (4) - 0
	//			RA5 (5) - 1
	porta = 0b00000000;
	trisa = 0b00101111;

	// Trisb:	CAN RX (3)	 - 1
	//			CAN TX (2)	 - 0
	//	DISCHARGE RELAY(5)	 - 0
	//     CHARGE RELAY(4)   - 0
	// 		THERMOSTAT (1)	 - 1
	portb = 0b00000000;
	trisb = 0b10001011;

	// Trisc: 	Serial RX(7) - 1
	//			Serial TX(6) - 0
	//			Buzzer (2) 	 - 0
	portc = 0b000000000;
	trisc = 0b10000000;

	// Initialize porta, portb, portc
	porta = 0x00;
	portb = 0x01;
	portc = 0x00;

#if WDT_ENABLED==1
	// Enable watchdog timer
	wdtcon = 00000001b;
#else
	#warning "Watchdog timer is disabled."
	// Disable watchdog timer
	wdtcon = 00000000b;
#endif

	// Initalize global variables
	ms10_tick = 0;
	ampere_tick = 0;
	first_cell = 0;
	first_mesg = 0;
	first_dtc = 0;
	lowest_ptr = 0;
	battery_soc = 0;
	battery_voltage = 0;
	battery_ampsecs = 0;
	timer_relay = 0;
	operation_mode = OPMODE_NORMAL;
	requested_opmode = OPMODE_NORMAL;
	full_run = false;
	charging = false;
	discharging = false;
	restart_collection = false;
	calibrate_sensor = true; // Automatically calibrate upon startup
	start_balancing = false;
	done_balancing = false;
	discharge_contactor = false;
	charge_contactor = false;

	// Initialize communication variables
	rxptr = 0;
	rxlength = 0;
	rxescape = 0;
	rxblock_bit = false;

	// Initialize error codes and related variables
	init_dtc();
	comm_errors = 0;
	cell_errors = 0;

	/* 
	 * Initialize the CAN Interface for the Prius's 500Kbps bus and a 20.000 Mhz crystal:
	 * 
	 * Baud rate registers
	 * Fosc = 20.000 MHz
	 * TQ = (2 * (BRP_ + 1))/FOSC = 2 * 1 / 20 MHz = 100 ns
	 * Nominal CAN bit rate for Prius = 500 KHz; Tbit = 2 us
	 * Therefore, Nominal Bit Time = 2 us / 100 ns = 20 TQ
	 * We divide it as follows:
	 *   Sync: 		 1 TQ (this is fixed)
	 *   Baud Rate Prescaler 0 BRP
	 *   Propagation: 	 3 TQ (the lines lengths are short, so that should be fine) PRSEG
	 *   Phase Segment 1:	 8 TQ (half of the remainder) SEG1PH
	 *   
	 * CANInitialize(SJW, BRP, PHSEG1, PHSEG2, PROPSEG, config);
	 */
	CANInitialize(1, 0, 8, 8, 3, CAN_CONFIG_VALID_STD_MSG); 

	// Initialize chip ID
	chip_id = 0;

	// Initialize regulator programmable values
	reg_adcdelay = eeprom_read(EEPROM_REG_ADCDELAY);
	reg_ledmode = eeprom_read(EEPROM_REG_LEDMODE);
	reg_safetytimer = (eeprom_read(EEPROM_REG_STIMER_HI) << 8) | eeprom_read(EEPROM_REG_STIMER_LO);
	reg_maxvoltage = (eeprom_read(EEPROM_REG_MAXVOLT_HI) << 8) | eeprom_read(EEPROM_REG_MAXVOLT_LO);
	
	// Initialize supervisor programmable values
//	num_cells = eeprom_read(EEPROM_NUMCELLS);
	num_cells = NUM_CELLS; // XXX
//	baudrate = eeprom_read(EEPROM_BAUDRATE);
	baudrate = BAUD_RATE; // XXX
//	shunting_enabled = eeprom_read(EEPROM_SHUNTING);
	shunting_enabled = SHUNTING; // XXX
//	shunt_duration = (eeprom_read(EEPROM_TIMERSHUNT_HI) << 8) | eeprom_read(EEPROM_TIMERSHUNT_LO);
	shunt_duration = TIMER_SHUNT; // XXX
//	pause_duration = (eeprom_read(EEPROM_TIMERPAUSE_HI) << 8) | eeprom_read(EEPROM_TIMERPAUSE_LO);
	pause_duration = TIMER_PAUSE; // XXX
//	min_voltage = (eeprom_read(EEPROM_MINVOLT_HI) << 8) | eeprom_read(EEPROM_MINVOLT_LO);
	min_voltage = MIN_VOLTAGE; // XXX
//	max_voltage = (eeprom_read(EEPROM_MAXVOLT_HI) << 8) | eeprom_read(EEPROM_MAXVOLT_LO);
	max_voltage = MAX_VOLTAGE; // XXX
//	diff_voltage = (eeprom_read(EEPROM_DIFFVOLT_HI) << 8) | eeprom_read(EEPROM_DIFFVOLT_LO);
	diff_voltage = DIFF_VOLTAGE; // XXX
//	balance_voltage = (eeprom_read(EEPROM_BALVOLT_HI) << 8) | eeprom_read(EEPROM_BALVOLT_LO);
	balance_voltage = BALANCE_VOLTAGE; // XXX
//	battery_ampfull = (eeprom_read(EEPROM_AMPERE_FULL_B0) << 8) | eeprom_read(EEPROM_AMPERE_FULL_B1);
//	battery_ampfull = (battery_ampfull << 16) | (eeprom_read(EEPROM_AMPERE_FULL_B2) << 8) | eeprom_read(EEPROM_AMPERE_FULL_B3);
	battery_ampfull = 0; // XXX
//	battery_ampempty = (eeprom_read(EEPROM_AMPERE_EMPTY_B0) << 8) | eeprom_read(EEPROM_AMPERE_EMPTY_B1);
//	battery_ampempty = (battery_ampempty << 16) | (eeprom_read(EEPROM_AMPERE_EMPTY_B2) << 8) | eeprom_read(EEPROM_AMPERE_EMPTY_B3);
	battery_ampempty = 0; // XXX
	
	// If we have an unconfigured value, suspend execution until we get values
	while(num_cells == 0xFF
	|| baudrate == 0xFF
	|| shunting_enabled == 0xFF
	|| shunt_duration == 0xFFFF
	|| pause_duration == 0xFFFF
	|| min_voltage == 0xFFFF
	|| max_voltage == 0xFFFF
	|| diff_voltage == 0xFFFF
	|| balance_voltage == 0xFFFF
	|| battery_ampfull == 0xFFFFFFFF
	|| battery_ampempty == 0xFFFFFFFF) {
		// Clear watchdog timer
		clear_wdt();

		// Configuration error
		toggle_bit(localporta, LED_ERROR);
		porta = localporta;
		delay_ms(50);
	}

	// Initialize cached values
//	battery_soc = eeprom_read(EEPROM_SOC);
	battery_soc = 0; // XXX
//	battery_amphours = (eeprom_read(EEPROM_AMPERE_CUR_B0) << 8) | eeprom_read(EEPROM_AMPERE_CUR_B1);
//	battery_amphours = (battery_amphours << 16) | (eeprom_read(EEPROM_AMPERE_CUR_B2) << 8) | eeprom_read(EEPROM_AMPERE_CUR_B3);
	battery_amphours = 0; // XXX
//	battery_amperr = (eeprom_read(EEPROM_AMPERE_ERR_HI) << 8) | eeprom_read(EEPROM_AMPERE_ERR_LO);
//	battery_amperr = 0; // XXX
/*
	// If battery_amperr is uninitialized then perform sensor calibration
	if(battery_amperr == 0xFFFF) {
		battery_amperr = 0;
		calibrate_sensor = true;
	} else {
		// Multiply to avoid floating point
		battery_amperr *= 10000; 
	}
*/

	// If battery_amphours is greater than full capacity, set it to full
	if(battery_amphours > battery_ampfull) {
		battery_amphours = battery_ampfull;
	}
	
	/*
	 * Serial_init - Initialize the serial interface
	 * Arguments: Baudrate scaler
	 */
	if(baudrate == 1) {
		serial_init(64);  // 9600 Baud at 40MHz
	} else if(baudrate == 2) {
		serial_init(32);  // 19200 Baud at 40MHz
	}

	// Initialize timer variables
	timer_shunt = 0;
	timer_pause = pause_duration;

	// Initialize shadows
	localporta = porta;
	localportb = portb;
	localportc = portc;
	localtrisa = trisa;
	localtrisb = trisb;
	localtrisc = trisc;

	// Initialize LED battery meter
	ledstate = 0;
	ledscale = 0;
	clear_bit(localportc, LED_METER1);
	clear_bit(localportc, LED_METER2);
	clear_bit(localportc, LED_METER3);
	clear_bit(localportc, LED_METER4);
	porta = localporta;
	portc = localportc;

	// Calibrate current sensor error percentage
	calibrate_sensor();

	// Load high priority DTCs from EEPROM
	load_dtcs(true);

	// Set interrupt priorities
	ipr1 = 0b00100000;			// Serial receive (bit 5) as high priority, all else low
	ipr2 = 0b00000000;			// All interrupts low priority
	ipr3 = 0b00000011;			// CANBUS receive buffers 0,1 as high priority, all else low
	clear_bit(intcon2, TMR0IP);	// Timer0 interrupt - Low
	set_bit(intcon2, INTEDG1);	// External interrupt on pull high edge
	set_bit(rcon, IPEN); 		// Enable interrupt priorities

	// Initialize timer0
	tmr0h = TIMER0_PRELOAD_HI;	// Preload high byte for timer0: 100ms
	tmr0l = TIMER0_PRELOAD_LO;	// Preload high byte for timer0: 100ms
	t0con = 10010011b;			// 1:16 Prescaler
	set_bit(intcon, TMR0IE);

	// Initialize timer1
	tmr1h = TIMER1_PRELOAD_HI;	// Preload high byte for timer1: 10ms
	tmr1l = TIMER1_PRELOAD_LO;	// Preload high byte for timer1: 10ms
	t1con = 00010001b;			// 1:2 Prescaler
	set_bit(pie1, TMR1IE);

	// Enable system interrupts for receiving messages
	set_bit(pie3, RXB0IE);
	set_bit(pie3, RXB1IE);
	set_bit(intcon, GIE);
	set_bit(intcon, PEIE);

#if CLEARBUTTON_ENABLED==1
	// Enable clear DTC button interrupt
	clear_bit(intcon, INT0IF);
	set_bit(intcon, INT0IE);
#endif // CLEARBUTTON_ENABLED

	// Enable serial interrupt
	set_bit(pie1, RCIE);

	// Send wake-up call to regulator chips
	SerialSendMessage(ID_WAKEUP, 0x00, NMD, 0x00, false);
	delay_ms(5);

/*
unsigned short safety_timer = 1800;
unsigned short max_voltage = 3625;
BYTE adc_delay = 1;
BYTE led_mode = 0x00;
BYTE yu;

for(yu = 1; yu < 81; ++yu) {
clear_wdt();
NMD[0] = 1;
NMD[1] = led_mode;
SerialSendMessage(ID_PROGRAM_LEDS, yu, NMD, 2, true);
delay_ms(100);
}

for(yu = 1; yu < 81; ++yu) {
clear_wdt();
NMD[0] = 1;
NMD[1] = (max_voltage >> 8) & 0x00FF;
NMD[2] = (max_voltage & 0x00FF);
SerialSendMessage(ID_PROGRAM_CUTOFFS, yu, NMD, 3, true);
delay_ms(100);
}

for(yu = 1; yu < 81; ++yu) {
clear_wdt();
NMD[0] = 1;
NMD[1] = (safety_timer >> 8) & 0x00FF;
NMD[2] = (safety_timer & 0x00FF);
SerialSendMessage(ID_PROGRAM_TIMERS, yu, NMD, 3, true);
delay_ms(100);
}

for(yu = 1; yu < 81; ++yu) {
clear_wdt();
NMD[0] = 1;
NMD[1] = adc_delay;
SerialSendMessage(ID_PROGRAM_DELAY, yu, NMD, 2, true);
delay_ms(100);
}
*/

/*
clear_wdt();
delay_s(1);
*/
	// Main application loop
	while(1) {

		// If we are not in normal mode, do not poll cells
		if(operation_mode != OPMODE_NORMAL) {
			// Clear watchdog timer
			delay_ms(100);
			clear_wdt();
		} else { // Else, poll cells
			// Call for an AD conversion:
			NMD[0] = record_id;  // Set record ID
			SerialSendMessage(ID_START_VOLT, 0x00, NMD, 1, false);

			// Initialize lowest and highest voltages
			lowest_ptr = 0;
			highest_ptr = 0;
			lowest_voltage = 0xFFFF;
			highest_voltage = 0x0000;
			battery_voltage = 0;
			restart_collection = false;

			// Wait for the regulator boards to finish calculations
			delay_ms(100);

			// Loop through each cell, make sure we are in normal operation mode
			for(target_board = 1; 
				target_board < (num_cells + 1);
				target_board++) {
	
				// Clear watchdog timer
				clear_wdt();

				// Transmit request
				NMD[0] = record_id; // Message history ID
				
				// Send the message, check response
				if(SerialSendMessage(ID_VOLT, target_board, NMD, 1, true) == false) {
	
					// False indicates we timed out twice, start over in data collection
					restart_collection = true;
				}
								
				// If this value is true, start over in data collection
				if(restart_collection == true)
					break;
	
			} // End data collection
		}
				
		// Check if there are any pending config messages
		while(first_mesg != 0) {
		
			// Clear watchdog timer
			clear_wdt();

			// Determine message type and broadcast it
			if(first_mesg->type == MESG_SERIAL) {
			
				// If destination isn't specified, broadcast to all chips
				if(first_mesg->dest == 0x00) {

					// Loop until we don't have any errors						
					for(target_board = 1; target_board < (num_cells + 1); target_board++) {
						do {
							// Initialize error flag
							error_config = false;
							
							// Clear watchdog timer
							clear_wdt();

							// Reset operation mode timer
							timer_opmode = TIMER_OPMODE;
						
							// Send a serial message
							SerialSendMessage(first_mesg->mesg_id, target_board, 
											first_mesg->data, first_mesg->length, 
											first_mesg->rxblock);
											
						} while(error_config == true); 
					}
				} else {
					// Send a serial message
					SerialSendMessage(first_mesg->mesg_id, first_mesg->dest, first_mesg->data, first_mesg->length, first_mesg->rxblock);
				}
			} else if(first_mesg->type == MESG_CANBUS) {
				// Send a CANBUS message
				CANSendMessage( first_mesg->mesg_id, first_mesg->data, 1, CAN_TX_PRIORITY_3 
													& CAN_TX_STD_FRAME 
													& CAN_TX_NO_RTR_FRAME);
			}
			
			// Remove message after being sent
			mesg_ptr = first_mesg;
			first_mesg = mesg_ptr->next;
			free((void *) mesg_ptr);

			// Reset operation mode timer
			timer_opmode = TIMER_OPMODE;
		}

#if CANCUTOFF_ENABLED==1
		// Check if we have had too many CANBUS errors
		if(rxerrcnt + txerrcnt > MAX_CAN_ERRORS
		&& error_canerrors == false) {
			// Flag the error
			error_canerrors = true;
			
			// Build message for DTC
			NMD[0] = ERROR_CAN_ERRORS;
			NMD[1] = DTC_PRIORITY_MED;
			NMD[2] = rxerrcnt;
			NMD[3] = txerrcnt;
			
			// Update the DTC, if it doesn't exist it will insert a new one
			update_dtc(4, NMD);
		}
#endif

		// Verify memory integrity: lowest voltage
		if(lowest_ptr->voltage != lowest_voltage
		&& lowest_voltage < 0xFFFF
		&& error_memfault == false) {
			// If the data structure doesn't line up with gathered results, memfault error
			error_memfault = true;

			// Build message for DTC
			NMD[0] = ERROR_MEM_FAULT;
			NMD[1] = DTC_PRIORITY_MED;
			NMD[2] = 0;
			NMD[3] = (lowest_voltage >> 8) & 0xFF;
			NMD[4] = lowest_voltage & 0xFF;
			NMD[5] = (lowest_ptr->voltage >> 8) & 0xFF;
			NMD[6] = lowest_ptr->voltage & 0xFF;
			
			// Update the DTC, if it doesn't exist it will insert a new one
			update_dtc(7, NMD);
		}		

		// Verify memory integrity: highest voltage
		if(highest_ptr->voltage != highest_voltage
		&& highest_voltage > 0x0
		&& error_memfault == false) {
			// If the data structure doesn't line up with gathered results, memfault error
			error_memfault = true;

			// Build message for DTC
			NMD[0] = ERROR_MEM_FAULT;
			NMD[1] = DTC_PRIORITY_MED;
			NMD[2] = 1;
			NMD[3] = (highest_voltage >> 8) & 0xFF;
			NMD[4] = highest_voltage & 0xFF;
			NMD[5] = (highest_ptr->voltage >> 8) & 0xFF;
			NMD[6] = highest_ptr->voltage & 0xFF;
			
			// Update the DTC, if it doesn't exist it will insert a new one
			update_dtc(7, NMD);
		}
	
		// Verify regulator cell integrity
		if(comm_errors >= MAX_COMM_ERRORS
		|| cell_errors >= MAX_CELL_ERRORS) {
			error_critical = true;
		}
	
		// Increment record ID for next pass
		record_id = (record_id % (MESSAGE_HISTORY-1)) + 1;

		// Output registers have somehow been overridden, very dangerous
		if(trisa != localtrisa
		|| trisb != localtrisb
		|| trisc != localtrisc) {

			// Emegency DTC broadcast before resetting
			NMD[0] = ERROR_TRIS_FAULT;
			NMD[1] = DTC_PRIORITY_MED;
					
			// If we haven't already tripped the trisfault error condition, set DTC
			if(error_trisfault == false)
				// Insert / Update DTC
				update_dtc(2, NMD);

			// Note the error and attempt to restore registers
			error_trisfault = true;
			trisa = localtrisa;
			trisb = localtrisb;
			trisc = localtrisc;

			// Save critical data to EEPROM		
//			eeprom_write(EEPROM_AMPERE_0, ((battery_amphours >> 24) & 0x00FF));
//			eeprom_write(EEPROM_AMPERE_1, ((battery_amphours >> 16) & 0x00FF));
//			eeprom_write(EEPROM_AMPERE_2, ((battery_amphours >> 8) & 0x00FF));
//			eeprom_write(EEPROM_AMPERE_3, (battery_amphours & 0x00FF));

			// Reset the chip to avoid any unpleasent surprises
			asm
			{
				RESET
			}
		}

		// Obtain recent copy of portb
		localportb = portb;

		// Initialize buzzer boolean
		buzzer_on = false;

		// Turn on buzzer and error LED high priority DTC detected
		for(dtc_ptr = first_dtc; dtc_ptr; dtc_ptr = dtc_ptr->next) {
			// Clear watchdog timer
			clear_wdt();

			// Only operate buzzer for HIGH priority DTCs
			if(dtc_ptr->data[1] == DTC_PRIORITY_HI) {

				/*
				* PWM registers configuration
				* see more details on http://www.micro-examples.com/
				*/	
				// Turn on the buzzer		
#if BUZZER_ENABLED==1
				pr2 = 0b11101010;
				t2con = 0b00000111;
				ccpr1l = 0b11101010;
				ccp1con = 0b00111100;
				buzzer_on = true;
#else
				#warning "Error buzzer is disabled."
#endif
			}
			
			// If DTC priority is greater than or equal to medium turn off contactors
			if(dtc_ptr->data[1] >= DTC_PRIORITY_MED) {
				// Toggle critical error boolean
				error_critical = true;

				// Set the error LED
				if(error_memfault == false && error_trisfault == false) {
					set_bit(localporta, LED_ERROR);
					porta = localporta;
				}
				
				// Turn off all shunting
				for(tmp_ptr = first_cell; tmp_ptr; tmp_ptr = tmp_ptr->next) {
						// If cell is shunting, stop it from shunting
						if(tmp_ptr->shunting == true) {
							NMD[0] = 0;
							SerialSendMessage(ID_SHUNT, tmp_ptr->id, NMD, 1, true);
						}
				}

				// Disrupt energy flow by tripping contactors
				charge_contactor = true;
				discharge_contactor = true;
			}

			// Only broadcast DTCs if we aren't in configuration mode
			if(operation_mode != OPMODE_CONFIG) {
															
				// Send the DTC CANBUS message
				CANSendMessage( CAN_ID_DTC, dtc_ptr->data, dtc_ptr->length, 
													CAN_TX_PRIORITY_3 
													& CAN_TX_STD_FRAME 
													& CAN_TX_NO_RTR_FRAME);
			
				// Delay between DTC messages
				delay_ms(1);
			}
		}
		
		// Turn buzzer off if we detected it wasn't necessary
		if(buzzer_on == false) {
			pr2 = 0x00;
			t2con = 0x00;
			ccp1con = 0x00;		
		}
		
		// Safety check to make sure these errors cut charging
		if(error_critical == true) {
			discharge_contactor = true;
			charge_contactor = true;
		}

		// Check to see if any cell's voltage has dipped below min value (min. discharge)
		if(lowest_voltage <= MIN_VOLTAGE 
		&& discharge_contactor == false
		&& lowest_voltage > 0) {
			// Set the stop discharge contactor on
			discharge_contactor = true;
		}
		
		// Check to see if any cell's voltage is higher than the max voltage (max charge)
		if(highest_voltage > MAX_VOLTAGE 
		&& charge_contactor == false) {
			// Set the charge contactor on
			charge_contactor = true;
		}

		// Adjust portb for contactors
		if(discharge_contactor == true 
		|| full_run == false
		|| operation_mode == OPMODE_CONFIG) {		
			// Disable the contactors (1 is on, 0 is off)
			clear_bit(localportb, DISCHARGE_CONTACTOR);
		} else {
			// Enable the contactors (1 is on, 0 is off)
			set_bit(localportb, DISCHARGE_CONTACTOR);
		}

		// Adjust portb for contactors
		if(charge_contactor == true 
		|| full_run == false
		|| operation_mode == OPMODE_CONFIG) {
			// Disable the contactors (1 is on, 0 is off)
			clear_bit(localportb, CHARGE_CONTACTOR);
		} else {
			// Enable the contactors (1 is on, 0 is off)
			set_bit(localportb, CHARGE_CONTACTOR);
		}

		// Set real portb to local copy
		portb = localportb;

		// If we restarted the collection gathering up at the top, continue
		if(restart_collection == true)
			continue;

		// Flag that we have made it through all regulators at least once
		full_run = true;

// XXX - Update battery SOC
// Take into account: battery voltage, current
		// Until we have a real way to do this, just guestimate
		if(charging == true || charge_contactor == true) {

			battery_soc = 0;
			
			if(lowest_voltage >= 3000
			&& lowest_voltage < 0xFFFF) {

				battery_soc = 25;
			
				if(lowest_voltage >= BALANCE_VOLTAGE)
					battery_soc = 50;

				if(highest_voltage >= 3500)
					battery_soc = 75;
			}

			// If we tripped the charging contactor, we're full
			if(charge_contactor == true && discharge_contactor == false) {
				battery_soc = 100;
			}
		} else {
			battery_soc = 0;

			if(lowest_voltage < 0xFFFF) {
				if(lowest_voltage >= 3000)
					battery_soc = 25;
			
				if(lowest_voltage >= 3200)
					battery_soc = 50;
				
				if(lowest_voltage >= 3300)
					battery_soc = 75;
			}
		}
// End XXX - SOC
		
		// Reduce accuracy of battery pack voltage for 16bit transit
		volt_bcast = battery_voltage / 100;

		// Build first 100ms broadcast message
		// [Highest Voltage] [Highest ID] [Lowest Voltage] [Lowest ID] [BMS Opmode]
		bcast1_100ms[0] = (BYTE) (highest_voltage >> 8) & 0x00FF;
		bcast1_100ms[1] = (BYTE) (highest_voltage) & 0x00FF;
		bcast1_100ms[2] = (BYTE) highest_ptr->id;
		bcast1_100ms[3] = (BYTE) (highest_voltage >> 8) & 0x00FF;
		bcast1_100ms[4] = (BYTE) (highest_voltage) & 0x00FF;
		bcast1_100ms[5] = (BYTE) highest_ptr->id;
		bcast1_100ms[6] = (BYTE) operation_mode;

		// Calculate checksum
		checksum = CAN_ID_BCAST1 + 0x07;
		for(i = 0; i < 7; ++i)
			checksum += bcast1_100ms[i];
		checksum &= 0xFF;
		bcast1_100ms[7] = (BYTE) checksum;
		
		// Build second 100ms broadcast message
		// [Pack Voltage] [SOC] [High Temp] [Low Temp] [Charger Relay] [Discharge Relay] 
		bcast2_100ms[0] = (BYTE)((volt_bcast >> 8) & 0x00FF);
		bcast2_100ms[1] = (BYTE)((volt_bcast) & 0x00FF);
		bcast2_100ms[2] = (BYTE) battery_soc;
		bcast2_100ms[3] = (BYTE) 0x00; // Future spot for highest temperature value
		bcast2_100ms[4] = (BYTE) 0x00; // Future spot for lowest temperature value
		bcast2_100ms[5] = (BYTE) charge_contactor;
		bcast2_100ms[6] = (BYTE) discharge_contactor;

		// Calculate checksum
		checksum = CAN_ID_BCAST2 + 0x07;
		for(i = 0; i < 7; ++i)
			checksum += bcast2_100ms[i];
		checksum &= 0xFF;
		bcast2_100ms[7] = (BYTE) checksum;

#if SHUNTING==1
		// If these conditions are met, start balancing
		if(charging == true
		&& shunting_enabled == true
		&& start_balancing == false
		&& highest_voltage >= BALANCE_VOLTAGE) {
			// Initialize the pause timer and set balancing flag
			timer_pause = pause_duration;
			start_balancing = true;
		}
		
		// Initialize shunting flags
		shunt_sent = false;
		shunt_errors = false;

		// Conditions to perform a shunting analysis
		if(shunting_enabled == true
		&& timer_shunt <= 0
		&& start_balancing == true
		&& error_critical == false) {
		
			// Clear watchdog timer
			clear_wdt();

			// For every one that is DIFF_VOLTAGE or more higher we shunt.
			for(tmp_ptr = first_cell; tmp_ptr; tmp_ptr = tmp_ptr->next) {
				// Clear watchdog timer
				clear_wdt();

				// Check if tmp_ptr->voltage is more than DIFF_VOLTAGE higher
				//   or if it is higher than MAX_VOLTAGE
				// Make sure we're not inbetween shunting evaluations.
				if((tmp_ptr->voltage - lowest_voltage > DIFF_VOLTAGE
					|| tmp_ptr->voltage >= MAX_VOLTAGE)
				&& timer_pause <= 0
				&& tmp_ptr->voltage > MIN_VOLTAGE) {

					// Send shunting request
					if(tmp_ptr->shunting == false) {
						NMD[0] = 1;

						// Trip flag so we know balancing is not done
						shunt_sent = true;

						if(SerialSendMessage(ID_SHUNT, tmp_ptr->id, NMD, 1, true) == false) {
							// False indicates an error during communications, flag it
							shunt_errors = true;
						}
					}
				} else {
					// We're done shunting
					if(tmp_ptr->shunting == true) {
						NMD[0] = 0;

						if(SerialSendMessage(ID_SHUNT, tmp_ptr->id, NMD, 1, true) == false) {
							// False indicates an error during communications, flag it
							shunt_errors = true;
						}
					}
				}
			}

			// Reset shunting related timers if no errors detected
			if(timer_pause <= 0 && shunt_errors == false) {
				// Only reset the shunting duration timer if we actually sent a request
				if(shunt_sent == true) {
					timer_shunt = shunt_duration;
				}

				// If these conditions are met, we are done balancing
				if(shunt_sent == false
				&& charge_contactor == true
				&& charging == false
				&& shunt_errors == false
				&& start_balancing == true) {
					// Set flags
					done_balancing = true;
					start_balancing = false;
					
					// Disable battery polling
					requested_opmode = OPMODE_NOPOLL;
				}

				timer_pause = pause_duration;
			}
		}
#else
	#warning "Battery balancing (shunting) is disabled."
#endif
	} // Do this forever
	
	return;
}

void calibrate_sensor() {
	unsigned long calibrate_sum = 0;
	short calibrate_var = 0;
	BYTE i;
	
	// Disable interrupts
	do {
		clear_bit(intcon, GIE);
	} while(intcon.GIE == 1);

	// Wait for voltage to become steady
	clear_wdt();
	delay_s(5);
	clear_wdt();
	
	// Initialize variable
	battery_amperr = 0;

	// Take 5 readings and average them (for accuracy)
	for(i = 0; i < 5; ++i) {

		// Delay between each evaluation
		clear_wdt();
		delay_ms(100);
		clear_wdt();
	
		// Poll ADC
		adcon0 &= 11000011b;
		adcon0 |= (0x00 & 0x15) << 2;
		delay_10us(2);
		adc_go = 1;
		while (adc_go); // Wait for ADC to finish
		calibrate_var = (short)adresh;
		calibrate_var = calibrate_var << 8;
		calibrate_var |= adresl;
		
		// Add ADC to sum
		calibrate_sum += calibrate_var;
	}

	// Take average of values
	calibrate_sum /= 5;
	
	// Set the amperror coefficient to the current ADC value, multiply by 10,000 to avoid floating point
	battery_amperr = calibrate_sum;
	battery_amperr = battery_amperr * 10000;
	
	// Re-enable interrupts
	do {
		set_bit(intcon, GIE);
	} while(intcon.GIE == 0);
}

char eeprom_read(char address)
{
	// Wait for write to complete
	while( eecon1.WR == 1);

	// Load address
	eeadr = address;

	// Point to EEPROM memory
	eecon1.EEPGD = 0;
	
	// 18F devices also need the CFGS bit to be cleared
#ifdef _PIC18
	eecon1.CFGS = 0;
#endif
	
	// Read
	eecon1.RD = 1;
	
	// Return value
	return eedata;
}

void eeprom_write(char address, char data)
{
	// 18F devices also need the CFGS bit to be cleared
#ifdef _PIC18
	eecon1.CFGS = 0;
#endif

	// Wait for write to complete
	while( eecon1.WR );

	clear_wdt();

	// Load address and data
	eeadr = address;
	eedata = data;

	// Point to EEPROM memory
	eecon1.EEPGD = 0;
	// Write enable
	set_bit(eecon1, WREN);
	// Disable interrupts
	do {
		clear_bit(intcon, GIE);
	} while(intcon.GIE == 1);

	// Required write sequence
	eecon2 = 0x55;
	eecon2 = 0xAA;
	set_bit(eecon1, WR);

	// Disable write
	clear_bit(eecon1, WREN);
		
	// Enable interrupts
	do {
		set_bit(intcon, GIE);
	} while(intcon.GIE == 0);
}
